#!/usr/bin/env python3

# Software License Agreement (BSD License)
# Copyright © 2014, 2022-2023 belongs to Shadow Robot Company Ltd.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification,
# are permitted provided that the following conditions are met:
#   1. Redistributions of source code must retain the above copyright notice,
#      this list of conditions and the following disclaimer.
#   2. Redistributions in binary form must reproduce the above copyright notice,
#      this list of conditions and the following disclaimer in the documentation
#      and/or other materials provided with the distribution.
#   3. Neither the name of Shadow Robot Company Ltd nor the names of its contributors
#      may be used to endorse or promote products derived from this software without
#      specific prior written permission.
#
# This software is provided by Shadow Robot Company Ltd "as is" and any express
# or implied warranties, including, but not limited to, the implied warranties of
# merchantability and fitness for a particular purpose are disclaimed. In no event
# shall the copyright holder be liable for any direct, indirect, incidental, special,
# exemplary, or consequential damages (including, but not limited to, procurement of
# substitute goods or services; loss of use, data, or profits; or business interruption)
# however caused and on any theory of liability, whether in contract, strict liability,
# or tort (including negligence or otherwise) arising in any way out of the use of this
# software, even if advised of the possibility of such damage.

from math import radians
from builtins import input
import rospy
from actionlib import SimpleActionClient, GoalStatus
from sr_robot_msgs.msg import GraspAction, GraspGoal
from sr_hand.shadowhand_ros import ShadowHand_ROS
from sr_grasp import Grasp, GraspStash


class QuickGraspNode:

    def __init__(self):
        self.stash = GraspStash()
        # self.stash.load_yaml_file("grasps.yaml")
        self.stash.load_all()
        if self.stash.size() == 0:
            rospy.logerr("No grasps found!")
            raise Exception("No Grasps")
        self.grasp = self.stash.get_all()[0]
        # print self.grasp
        rospy.loginfo("Looking for hand...")
        self.hand = ShadowHand_ROS()
        rospy.loginfo("Found")

    def zero_hand(self):
        for joint in self.hand.allJoints:
            self.hand.sendupdate(joint.name, 0.0)
        rospy.sleep(2)

    def run_grasp(self, pre=False):
        goal = GraspGoal()
        goal.grasp = self.grasp
        goal.pre_grasp = pre
        client = SimpleActionClient('grasp', GraspAction)
        client.wait_for_server()
        client.send_goal(goal)
        client.wait_for_result(rospy.Duration.from_sec(20.0))
        if client.get_state() == GoalStatus.SUCCEEDED:
            rospy.loginfo("Success!")
        else:
            rospy.logerr("Fail!")

    def new_grasp(self, name):
        grasp = Grasp()
        grasp.id = name
        positions = self.hand.read_all_current_positions()
        positions = {key: radians(value) for key, value in positions.items()}
        grasp.set_grasp_point(positions=positions)
        self.stash.put_grasp(grasp)
        node.grasp = grasp


if __name__ == '__main__':
    rospy.init_node("quick_grasp")
    node = QuickGraspNode()
    while node:
        print("")
        print("Grasps:")
        for i, grasp_instance in enumerate(node.stash.get_all()):
            print(f"{i} - {grasp_instance.id}")
        print(f"Current grasp: {node.grasp.id}")
        print("Number select grasp, z zero hand, g grasp, p pre-grasp, n new grasp, s save grasps, q quit")
        act = input(": ")
        num = None
        try:
            num = int(act)
        except ValueError:
            pass
        if num is not None:
            node.grasp = node.stash.get_grasp_at(num)
            act = 'p'  # auto go to pre grasp on grasp select

        if act == "z":
            node.zero_hand()
        elif act == "p":
            node.run_grasp(pre=True)
        elif act == "g":
            node.run_grasp()
        elif act == "n":
            name_input = input("Enter name: ")
            node.new_grasp(name=name_input)
        elif act == "s":
            node.stash.save_yaml_file()
        elif act == "q":
            break
        else:
            print(f"Unknown action {act}")
